/*
author: Wu Zhen
email: zhen.wu@gameloft.com
version: 1.0.0
*/
#include "stdafx.h"

#include "math3d.h"

vector_t GetPosUniformLinear3D( const vector_t& v0, 
											const float time,
											const vector_t& startPos)
{
	return startPos + v0*time;
}

vector_t GetPosLinearInterpolation3D(const vector_t& startPos,
									               const vector_t& endPos,
												   const float totalTime,
												   const float currentTime)
{
	return  startPos + (endPos - startPos) * currentTime / totalTime;
}

vector_t GetPosUniformVariable3D(const vector_t& v0, 
											  const vector_t& acceleration,
											  const float time,
											  const vector_t& startPos)
{
	return startPos + v0*time + 0.5*acceleration*time*time;
}

vector_t GetPosUniformVariableLinear3D(const vector_t& moveDir, 
													  const float v0,
													  const float acceleration,
													  const float time,
													  const vector_t& startPos,
													  bool setSpeedLimit,
													  const float speedLimit)
{

	if(setSpeedLimit)
	{	
		float limitTime = (speedLimit - v0) / acceleration;
		if( time <= limitTime)
		{
			return startPos + (v0 * time + 0.5 * acceleration * time * time) * moveDir;
		}
		else
		{
			return startPos + (v0 * limitTime + 0.5 * acceleration * limitTime * limitTime + (time - limitTime) * speedLimit) *moveDir;
		}
	}
	else
	{
		return startPos + (v0 * time + 0.5 * acceleration * time * time)*moveDir;
	}

}

vector_t RotateAroundAxis(const vector_t& point,
						             const vector_t& axis,
									 float angle,
									 const vector_t& origin)
{
	vector_t vt = axis;
	vt.normalize();
	vector_t vt0 = vt;

	vector_t tv = point - origin;
	vt = tv.dotProduct(vt)*vt;
	vector_t vt1 = tv - vt;

	vector_t vt2 = vt1.crossProduct(vt0);

	vector_t result = vt + vt1*cos(angle) + vt2* sin(angle) + origin;

	return result;
}

vector_t GetPosUniformCircular3D(const vector_t& eyePos,
								              const vector_t& startPos,
											  const vector_t& circleOrigin,
											  float AngleSpeed,
											  const float time)
{	
	float rotateAngle = AngleSpeed * time;

	vector_t circleNormal = eyePos - circleOrigin;
	vector_t circleOriginToStart = startPos - circleOrigin;
	circleOriginToStart.normalize();
	circleNormal = circleNormal - circleOriginToStart*(circleOriginToStart.dotProduct(circleNormal));
	circleNormal.normalize();

	return RotateAroundAxis(startPos, circleNormal, rotateAngle, circleOrigin);

}

vector_t GetPosUniformCircular3D_2(const vector_t& axis,
								              const vector_t& startPos,
											  const vector_t& circleOrigin,
											  float AngleSpeed,
											  const float time)
{
	float rotateAngle = AngleSpeed * time;
	return RotateAroundAxis(startPos, axis, rotateAngle, circleOrigin);
}

vector_t GetVelocity2TargetUseingTrackingCurve2D(const vector_t& targetPos, 
							const vector_t& myPos, 
							const vector_t& lastVelocity,
							const float& angularAcc,
							const float& trackingMinDegree,
							const float& trackingMaxDegree,
							const float& time)
{
	vector_t newVelocity = lastVelocity;
	//
	irr::core::vector3df dir2Target = targetPos - myPos;
	dir2Target.Z = 0.f;
	dir2Target.normalize();
	irr::core::vector3df curDir = lastVelocity;
	curDir.Z = 0.f;
	curDir.normalize();
	//
	float cosValue = dir2Target.dotProduct(curDir);
	if(cosValue > 1)
		cosValue = 1;
	if(cosValue < -1)
		cosValue = -1;
	float angularLimit = acos(cosValue) * irr::core::RADTODEG;
	if(angularLimit >= trackingMinDegree && angularLimit <= trackingMaxDegree) {
		float rotAngle = angularAcc * time;
		if (rotAngle > angularLimit) {
			rotAngle = angularLimit;
		}
		irr::core::vector3df axis(-curDir.Y, curDir.X, 0.f);
		if(axis.dotProduct(dir2Target) < 0) {
			rotAngle = -rotAngle;
		}

		newVelocity.rotateXYBy(rotAngle);
	}

	return newVelocity;
}

vector_t GetVelocity2TargetUseingTrackingCurve3D(const vector_t& targetPos, 
							const vector_t& myPos, 
							const vector_t& lastVelocity,
							const float& angularAcc,
							const float& trackingMinDegree,
							const float& trackingMaxDegree,
							const float& time)
{
	vector_t newVelocity = lastVelocity;
	//
	irr::core::vector3df dir2Target = targetPos - myPos;
	dir2Target.normalize();
	irr::core::vector3df curDir = lastVelocity;
	curDir.normalize();
	//
	float cosValue = dir2Target.dotProduct(curDir);
	float angularLimit = acos(cosValue) * irr::core::RADTODEG;
	if(angularLimit > trackingMinDegree && angularLimit < trackingMaxDegree) {
		float rotAngle = angularAcc * time;
		if (rotAngle > angularLimit) {
			rotAngle = angularLimit;
		}
		irr::core::vector3df axis(-curDir.Y, curDir.X, 0.f);
		if(axis.dotProduct(dir2Target) < 0) {
			rotAngle = -rotAngle;
		}

		newVelocity.rotateXYBy(rotAngle);
	}

	return newVelocity;
}

vector_t GetMapPoint(const vector_t& pointInA,
					          const int veticeCount,
							  vector_t* veticeA,
							  vector_t* veticeB)
{
	int currentVetex = 1;
	vector_t dir = pointInA - veticeA[0];
	while(currentVetex < (veticeCount - 2))
	{
		vector_t leftJudgeDir = (veticeA[currentVetex] - veticeA[0]).crossProduct(pointInA - veticeA[0]);
		vector_t rightJudgeDir = (veticeA[currentVetex + 1] - veticeA[0]).crossProduct(pointInA - veticeA[0]);
		if(leftJudgeDir.dotProduct(rightJudgeDir) >=0)break;
		++currentVetex;
	}
	vector_t uDir = veticeA[currentVetex] - veticeA[0];
	vector_t vDir = veticeA[currentVetex + 1] - veticeA[0];
	float u =  (dir.X * vDir.Y - dir.Y * vDir.X) / (uDir.X * vDir.Y - uDir.Y * vDir.X);
	float v = (dir.X - u*uDir.X)/vDir.X;

	vector_t result = u*veticeB[currentVetex] + v*veticeB[currentVetex + 1] + (1-u-v)*veticeB[0];
	return result;
}

int  GetRayIntersectionInXY(const vector_t& pointA, 
							             const vector_t& dirA,
							             const vector_t& pointB,
										  const vector_t& dirB,
							             float&   disInA,
										 float&   disInB,
										 float    rayRadius)
{
	vector_t dir_A = dirA;
	dir_A.Z = 0;
	dir_A.normalize();

	vector_t dir_B = dirB;
	dir_B.Z = 0;
	dir_B.normalize();	
	
	vector_t AtoB = pointB - pointA;
	AtoB.Z = 0;
	

	float judgeColine = dir_A.dotProduct(dir_B);
	if(dir_A.getLength( )<0.001 ||dir_B.getLength( )<0.001)
	{
		judgeColine = 1;
	}
		if(judgeColine > 0.999 )
		{		
			if(AtoB.getLength() < 0.001)
			{
				return 2;
			}
			else
			{
				AtoB -=  AtoB.dotProduct(dir_A)*dir_A;
				if(AtoB.getLength() < rayRadius)
				{
					return 2;
				}
				else
				{
					return -1;
				}
			}
		}
		else if(judgeColine < -0.999 )
		{
			if(AtoB.getLength() < 0.001)
			{
				//disInA = 0;
				//disInB = 0;
				//return 1;
				return -2;
			}
			else
			{
				AtoB.normalize();
				float judgeColine = AtoB.dotProduct(dir_A);
				if(judgeColine > 0.999 )
				{
					return -2;
				}
				else
				{
					return -1;
				}
			}
		}

		float Ax = pointA.X;
		float Ay = pointA.Y;
		float Bx = pointB.X;
		float By = pointB.Y;

		disInB = (dir_A.Y * (Ax - Bx) - dir_A.X * (Ay - By)) / (dir_B.X * dir_A.Y - dir_B.Y * dir_A.X);	
		disInA =  ((Bx - Ax) + disInB * dir_B.X) / dir_A.X;

		if(disInB < 0 || disInA < 0)
		{
			return -1;
		}
		else
		{
			return 1;
		}
}

bool IsPointInsideBox(const vector_t& point,
					         const vector_t& minButtomCenter,
							 const vector_t& forward,
							 const vector_t& up,
							 float  halfWidth)
{
	vector_t OToP = point - minButtomCenter;
	
	float judgeShadow = OToP.dotProduct(forward);
	if(judgeShadow < 0 || judgeShadow > forward.getLengthSQ())
	{
		return false;
	}

	judgeShadow = OToP.dotProduct(up);
	if(judgeShadow < 0 || judgeShadow > up.getLengthSQ())
	{
		return false;
	}

	vector_t side = forward.crossProduct(up);
	side.normalize();
	judgeShadow = OToP.dotProduct(side);
	if(judgeShadow > halfWidth || judgeShadow < -halfWidth)
	{
		return false;
	}

	return true;
}

template<class T>
T GetValueOnCubicBezier(T pStart,T pControl,T pEnd, float t)
{
	return (1-t)*(1-t)*pStart + 2*t*(1-t)*pControl + t*t*pEnd;
}

float GetYOnCubicBezier2D( float startX, float startY, float controlX,float controlY, float endX, float endY, float x)
{
	if(x > endX && x > startX)
	{
		if(endX > startX)
			return endY;
		else
			return startY;
	}
	else if(x < endX && x < startX)
	{
		if(endX > startX)
			return startY;
		else
			return endY;
	}
	float t;
	if(startX-2*controlX+endX == 0)
	{
		t = (x - startX) * 0.5 / (controlX - startX);
	}
	else
	{
		t=((startX-controlX)+sqrt((controlX-startX)*(controlX-startX)-(startX-x)*(startX-2*controlX+endX)))/(startX-2*controlX+endX);
		if(t < 0 || t > 1)
		{
			t = ((startX-controlX)-sqrt((controlX-startX)*(controlX-startX)-(startX-x)*(startX-2*controlX+endX)))/(startX-2*controlX+endX);
		}
	}
	return GetValueOnCubicBezier<float>(startY,controlY,endY,t);
}